/* IMPLEMENATION OF K-MEANS FOR PURE DATA: UNSUPERVISIONED MACHINE LEARNING ALGORITHM
 * FOR DATA CLUSTERIZATION.
 *  
 *  by Adriano Monteiro, december, 27, 2012. contact: monteiro.adc@gmaill.com
 *  
 */

#include "m_pd.h"
#include <stdlib.h>
#include <math.h>


static t_class *filter_class;

typedef struct _filter {
        t_object x_obj;
        
  
        // leitura da array source
        t_word *src_vec;
        t_symbol *src_arrayname;
    	int src_array_points;
    	
        // leitura da array destine
        t_word *dst_vec;
        t_symbol *dst_arrayname;
       	int dst_array_points;
                
        // buffer
       	double *B;
       	t_float size_of_B;
       	double *A;
       	t_float size_of_A;
        
        // outlets
        //t_outlet *x_list;
        
                    
        
}t_filter;

/*********************** INTERNAL FUNCTIONS  *******************************/

void filter_coefB(t_filter *x, t_symbol *s, int argc, t_atom *argv){
	
	int i;
	
	if(x->size_of_B)freebytes(x->B, x->size_of_B * sizeof(double));
	x->size_of_B = argc;	
	x->B = (double  *)getbytes(argc* sizeof(double));
	
	for(i=0; i<argc; i++)x->B[i] = atom_getfloat(argv+i); 
	
}

void filter_coefA(t_filter *x, t_symbol *s, int argc, t_atom *argv){
	
	int i;
		
	if(x->size_of_A)freebytes(x->A, x->size_of_A * sizeof(double));
	x->size_of_A = argc;	
	x->A = (double *)getbytes(argc* sizeof(double));
		
	for(i=0; i<argc; i++)x->A[i] = atom_getfloat(argv+i);
	
}


void filter_normalize_coef(double *vectorA, double *vectorB, t_floatarg size,  double coef){
	
	int i;
	double max=0 ;
	
	if(coef!= 1){	
		for(i=0; i<size; i++){
			if(max < vectorA[i]) max = vectorA[i];
			else if(max < vectorB[i]) max = vectorB[i];
			}
		for(i=0; i<size; i++){ vectorA[i] = (vectorA[i]/max)*coef; vectorB[i] = (vectorB[i]/max)*coef;}
		}
}

/*


void wav2aud_filter(t_floatarg *y, t_floatarg *wav, t_floatarg *B, t_floatarg *A,  t_floatarg size, t_floatarg size_coef){ 
	int i,j;	
	float filt, *buffer, *buffer2;
	
	post("A0:%f", A[0]);
	wav2aud_normalize_coef(A, B, size_coef, A[0]);
	//wav2aud_normalize_coef(A, size_coef, A[0]);
	A[0] = 0;
	
	buffer = (float *)getbytes(size_coef*sizeof(float));
	buffer2 = (float *)getbytes(size_coef*sizeof(float));
	for(i = 0; i < size_coef ;i++){ buffer[i] = 0; buffer2[i] = 0; } 
	
	
	for(i= 0; i < size; i++){
		//post("************************************************");
		for(j=size_coef-1; j > 0; j--)buffer[j] = buffer[j-1]; 
		buffer[0]=wav[i]; 
		filt =0;
		for(j=0; j < size_coef; j++) filt = filt + buffer[j]*B[j] - buffer2[j]*A[j]; 
		for(j=size_coef-1; j > 0 ; j--){buffer2[j] = buffer2[j-1]; post("B[%d] = %f",j, buffer2[j]);} 
		buffer2[0]=filt; post("B[0] = %f, filt = %f", buffer2[0],filt);
		y[i] = filt;
		
		
	}
	
	freebytes(buffer, size*sizeof(float));
	freebytes(buffer2, size*sizeof(float));
	
	
} */

// based on http://mechatronics.ece.usu.edu/yqchen/filter.c/FILTER.C   from    http://mechatronics.ece.usu.edu/yqchen/filter.c/    

void filter_filter(double *y, double *wav, double *B, double *A,  t_floatarg size, t_floatarg size_coef){ 
	
	int i,j;	
	
	filter_normalize_coef(A, B, size_coef, A[0]);
	
	y[0]=B[0]*wav[0];
	for (i=1;i<size_coef;i++){
	    y[i]=0.0;
	    for(j=0;j<i+1;j++)y[i]=y[i]+B[j]*wav[i-j];
	    for(j=0;j<i;j++) y[i]=y[i]-A[j+1]*y[i-j-1];
	}
	for (i=size_coef;i<size;i++){
		y[i]=0.0;
	    for (j=0;j<size_coef;j++)y[i]=y[i]+B[j]*wav[i-j];
	    for (j=0;j<size_coef-1;j++)y[i]=y[i]-A[j+1]*y[i-j-1];
	}
	
	
} 

/******************* BANG MESSAGE ***************************************/

void filter_bang(t_filter *x) {
	
	int i;
	double *wav, *y;
	t_garray *a, *b;

	// access array
	if(!(a = (t_garray *)pd_findbyclass(x->src_arrayname, garray_class)))
	  	pd_error(x, "%s: no such array", x->src_arrayname->s_name);
	else if(!garray_getfloatwords(a, &x->src_array_points, &x->src_vec))
		pd_error(x, "%s: bad template for wav2aud", x->src_arrayname->s_name);	
	
	if(!(b = (t_garray *)pd_findbyclass(x->dst_arrayname, garray_class)))
		pd_error(x, "%s: no such array", x->dst_arrayname->s_name);
	else if(!garray_getfloatwords(b, &x->dst_array_points, &x->dst_vec))
		pd_error(x, "%s: bad template for wav2aud", x->dst_arrayname->s_name);	
	
	wav = (double *)getbytes(x->src_array_points * sizeof(double));		// buffer for array data (original signal)
	y = (double *)getbytes(x->src_array_points * sizeof(double));		// buffer for filtered signal (last channel)
	//for(i=0; i<x->src_array_points; i++) y[0] = 0;
						 
	for(i=0; i < x->src_array_points; i++)wav[i] = x->src_vec[i].w_float;  // copy array
		
	filter_filter(y, wav, x->B, x->A, x->src_array_points, x->size_of_B);
	
	for(i=0; i < x->src_array_points; i++)x->dst_vec[i].w_float = y[i];
		
	garray_redraw(b);
			
	// free local memory
	freebytes(y1, x->src_array_points * sizeof(double));
	freebytes(wav, x->src_array_points * sizeof(double));
		
	
    
}

/****************************** DATA ACESS METHOD **********************************/


		
/************************** OBJECT CREATION AND SETUP *************************************/


void filter_free(t_filter *x){
	
	freebytes(x->A, x->size_of_A * sizeof(double));
	freebytes(x->B, x->size_of_B * sizeof(double));
	
}



void *filter_new(t_symbol *s, int argc, t_atom *argv){
     t_filter *x = (t_filter *)pd_new(filter_class);
     
  // acesso a array copiado do tabletool do brent 	
	t_garray *a, *b;
 		
  	if(atom_getsymbol(argv)){
  		x->src_arrayname = atom_getsymbol(argv);

  		if(!(a = (t_garray *)pd_findbyclass(x->src_arrayname, garray_class)))
  		        pd_error(x, "%s: no such array", x->src_arrayname->s_name);
  		 else if(!garray_getfloatwords(a, &x->src_array_points, &x->src_vec))
  		    	pd_error(x, "%s: bad template for wav2aud", x->src_arrayname->s_name);	
  		}
  		else
  			post("WARNING: no array specified.");
  		
  	if(atom_getsymbol(argv+1)){
  		 x->dst_arrayname = atom_getsymbol(argv+1);

  		 if(!(b = (t_garray *)pd_findbyclass(x->dst_arrayname, garray_class)))
  		        pd_error(x, "%s: no such array", x->dst_arrayname->s_name);
  		 else if(!garray_getfloatwords(b, &x->dst_array_points, &x->dst_vec))
  		     	pd_error(x, "%s: bad template for wav2aud", x->dst_arrayname->s_name);	
  		 }
  		 else
  		 		post("WARNING: no array specified.");
  		
  	// allocate memory for frame output as list
 	//x->frame_out = (t_atom *)getbytes(M * sizeof(t_atom));	
  	
  	x->size_of_A = 0;
  	x->size_of_B = 0;
  		
  	//cria inlets  	 
  	inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("list"), gensym("coefB"));
  	inlet_new(&x->x_obj, &x->x_obj.ob_pd, gensym("list"), gensym("coefA"));
  		
    // cria os outlets
  	//x->x_list = outlet_new(&x->x_obj, gensym("list")); 
     
     return (void *) x;
}

    
void filter_setup(void) {
     filter_class = class_new(gensym("filter"), (t_newmethod)filter_new, 
     (t_method)filter_free, sizeof(t_filter), CLASS_DEFAULT, A_GIMME, 0);
      
    //class_addlist(kmeans_class, kmeans_coefB);
    //class_addlist(kmeans_class, kmeans_coefA);
    class_addbang(filter_class, filter_bang);
    class_addmethod(filter_class, (t_method)filter_free, gensym("clear"),0);
    class_addmethod(filter_class, (t_method)filter_coefB, gensym("coefB"), A_GIMME, 0);
    class_addmethod(filter_class, (t_method)filter_coefA, gensym("coefA"), A_GIMME, 0);
 
}

